import pandas as pd
import numpy as np
import os

def interpolate_rows(df, num_insert=10):
    new_data = []
    for i in range(len(df) - 1):
        start_row = df.iloc[i]
        end_row = df.iloc[i + 1]

        # Append the start row
        new_data.append(start_row)

        # Generate interpolated rows
        for j in range(1, num_insert + 1):
            interpolated_row = start_row + (end_row - start_row) * (j / (num_insert + 1))
            new_data.append(interpolated_row)

    # Append the last row
    new_data.append(df.iloc[-1])
    
    # Convert the list of rows back to a DataFrame
    interpolated_df = pd.DataFrame(new_data, columns=df.columns)
    
    return interpolated_df

def convert_curv_to_circle(input_file, output_file):
    # Load the curv.csv file
    curv_df = pd.read_csv(input_file)

    # Rename the columns to px, py, pz
    curv_df.columns = ['px', 'py', 'pz']

    # Interpolate rows between each pair of rows
    interpolated_df = interpolate_rows(curv_df)

    # Calculate vx, vy, vz as differences of px, py, pz
    interpolated_df['vx'] = interpolated_df['px'].diff().fillna(0)
    interpolated_df['vy'] = interpolated_df['py'].diff().fillna(0)
    interpolated_df['vz'] = interpolated_df['pz'].diff().fillna(0)

    # Calculate ax, ay, az as differences of vx, vy, vz
    interpolated_df['ax'] = interpolated_df['vx'].diff().fillna(0)
    interpolated_df['ay'] = interpolated_df['vy'].diff().fillna(0)
    interpolated_df['az'] = interpolated_df['vz'].diff().fillna(0)

    # Create a DataFrame to hold the first row repeated 30 times with zero velocity and acceleration
    first_row_repeated = pd.DataFrame([interpolated_df.iloc[0]] * 30)
    first_row_repeated[['vx', 'vy', 'vz', 'ax', 'ay', 'az']] = 0

    # Concatenate the repeated first row DataFrame with the interpolated DataFrame
    final_df = pd.concat([first_row_repeated, interpolated_df], ignore_index=True)

    # Ensure the output directory exists
    os.makedirs(os.path.dirname(output_file), exist_ok=True)

    # Save the result to the specified output file without the header
    final_df.to_csv(output_file, index=False, header=False)

if __name__ == "__main__":
    input_file = 'curv.csv'
    output_file = '/home/up/sing_offb_ws/src/offboard/cfg/circle_r1m.csv'

    convert_curv_to_circle(input_file, output_file)
    print(f"File {output_file} has been created successfully.")

